#pragma once

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <autodiff/reverse.hpp>
#include <autodiff/reverse/eigen.hpp>

using namespace Eigen;
using namespace autodiff;

namespace photo {
constexpr double DEG2RAD = 0.01745329251994329576923690768489;
constexpr double RAD2DEG = 57.295779513082320876798154814105;

inline Matrix3d rodrigues(const Vector3d &r) {
    Eigen::AngleAxisd angle_axis(r.norm(), r.normalized());
    return angle_axis.toRotationMatrix();
}

inline Matrix3var rodrigues(const Vector3var &angle_axis) {
    static const double kOne = 1.0;
    const var theta2 = angle_axis.squaredNorm();
    Matrix3var R;
    if (theta2 > var(std::numeric_limits<double>::epsilon())) {
        // We want to be careful to only evaluate the square root if the
        // norm of the angle_axis vector is greater than zero. Otherwise
        // we get a division by zero.
        const var theta = sqrt(theta2);
        const var wx = angle_axis[0] / theta;
        const var wy = angle_axis[1] / theta;
        const var wz = angle_axis[2] / theta;

        const var costheta = cos(theta);
        const var sintheta = sin(theta);

        R(0, 0) = costheta + wx * wx * (kOne - costheta);
        R(1, 0) = wz * sintheta + wx * wy * (kOne - costheta);
        R(2, 0) = -wy * sintheta + wx * wz * (kOne - costheta);
        R(0, 1) = wx * wy * (kOne - costheta) - wz * sintheta;
        R(1, 1) = costheta + wy * wy * (kOne - costheta);
        R(2, 1) = wx * sintheta + wy * wz * (kOne - costheta);
        R(0, 2) = wy * sintheta + wx * wz * (kOne - costheta);
        R(1, 2) = -wx * sintheta + wy * wz * (kOne - costheta);
        R(2, 2) = costheta + wz * wz * (kOne - costheta);
    } else {
        // Near zero, we switch to using the first order Taylor expansion.
        R(0, 0) = kOne;
        R(1, 0) = angle_axis[2];
        R(2, 0) = -angle_axis[1];
        R(0, 1) = -angle_axis[2];
        R(1, 1) = kOne;
        R(2, 1) = angle_axis[0];
        R(0, 2) = angle_axis[1];
        R(1, 2) = -angle_axis[0];
        R(2, 2) = kOne;
    }
    return R;
}

inline Vector2d project(const Vector3d &point, const Vector3d &Xs, const Matrix3d &R, double f, double x0, double y0) {
    Vector2d pixel;

    Vector3d X = point - Xs;
    X = R.transpose() * X;

    pixel.x() = -f * X(0) / X(2) + x0;
    pixel.y() = -f * X(1) / X(2) + y0;

    return pixel;
}

inline Vector2d project(const Vector3d &point, const Vector3d &Xs, const double R[], double f, double x0, double y0) {
    Vector2d pixel;

    double X = point[0] - Xs[0];
    double Y = point[1] - Xs[1];
    double Z = point[2] - Xs[2];

    pixel.x() = -f * (R[0] * X + R[3] * Y + R[6] * Z) / (R[2] * X + R[5] * Y + R[8] * Z) + x0;
    pixel.y() = -f * (R[1] * X + R[4] * Y + R[7] * Z) / (R[2] * X + R[5] * Y + R[8] * Z) + y0;

    return pixel;
}

} // namespace photo
